#include "circle_system.h"

namespace Packing_common
{
		///////////////////////	Single Circles	//////////////////////
	Circle_system::Circle_system(const std::vector<Particle_circle>& particles, const Container_box& bndbox) : particle_sys(particles), System_potential_energy(bndbox)
	{ 	
		//System_potential_energy::;
		nonoverlap_constraint = true;
	}

	double Circle_system::molecular_energy_at(const double *const x)
	{
		double energy_sum = 0.0;
		for (unsigned int i = 0; i < particle_sys.size(); i++)
		{
			Point_2 ci = particle_sys[i].center() + Vector_2(x[2*i], x[2*i+1]);
			for (unsigned int j = i+1; j < particle_sys.size(); j++)
			{
				Point_2 cj = particle_sys[j].center() + Vector_2(x[2*j], x[2*j+1]);
		
				double rm = f*(particle_sys[i].radius() + particle_sys[j].radius());
				// (rm/||c0-c1||)^6
				double d2 = CGAL::squared_distance(ci, cj);
				double rm2_over_d2 = rm*rm/d2;
				double rm6_over_d6 = std::pow(rm2_over_d2, 3.0);

				// (rm/(r0+r1))^6
				energy_sum += rm6_over_d6*(rm6_over_d6 - 2) ;
			
			}
		}
		energy_sum *= 2*epsilon; // due to symmetry

		return energy_sum;
	}
	double Circle_system::gravity_energy_at(const double *const x)
	{
		double gravity_energy = 0.0;
		for (unsigned int i = 0; i < particle_sys.size(); i++)
		{
			Point_2 ci = particle_sys[i].center() + Vector_2(x[2*i], x[2*i+1]);
			gravity_energy += particle_sys[i].squared_radius()*(ci.y() - bounding_box.y_min()); 
		}
		return boost::math::constants::pi<double>()*g*gravity_energy;
	}
	void Circle_system::energy_gradient_at(const double *const x, double *gradient)
	{
		for (unsigned int i = 0; i < particle_sys.size(); i++)
		{
			gradient[2*i] = gradient[2*i+1] = 0.0;
			Point_2 ci = particle_sys[i].center() + Vector_2(x[2*i], x[2*i+1]);
			for (unsigned int j = 0; j < particle_sys.size(); j++)
			{
				if (j == i)	continue;

				Point_2 cj = particle_sys[j].center() + Vector_2(x[2*j], x[2*j+1]);
				// ||(ci+ti) - (cj+tj)||^2
				double d2 = CGAL::squared_distance(ci, cj);
				// (rm/||c0-c1||)^6
				double rm = f*(particle_sys[i].radius() + particle_sys[j].radius());
				double rm2_over_d2 = rm*rm/d2;
				double rm6_over_d6 = std::pow(rm2_over_d2, 3.0);

				double temp = 12*rm6_over_d6/d2;
				temp *= (ci.x() + x[2*i] - cj.x() - x[2*j]);
				temp *= (1- rm6_over_d6);
				gradient[2*i] += temp;

				temp = 12*rm6_over_d6/d2;
				temp *= (ci.y() + x[2*i+1] - cj.y() - x[2*j+1]);
				temp *= (1- rm6_over_d6);
				gradient[2*i+1] += temp;
			}
			// due to symmetry and the common factor
			gradient[2*i] *= 2*epsilon;
			gradient[2*i+1] *= 2*epsilon;
		}
		for (unsigned int i = 0; i < particle_sys.size(); i++)
		{
			gradient[2*i+1] += boost::math::constants::pi<double>()*particle_sys[i].squared_radius()*g;
		}
	}
	void Circle_system::constraint_at(const double *const x, double *constraint_values)
	{
		if (!nonoverlap_constraint)		return;
		for (unsigned int i = 0; i < particle_sys.size(); i++)
		{
			Point_2 ci = particle_sys[i].center() + Vector_2(x[2*i], x[2*i+1]);
			for (unsigned int j = i+1; j < particle_sys.size(); j++)
			{
				Point_2 cj = particle_sys[j].center() + Vector_2(x[2*j], x[2*j+1]);
				*constraint_values++ = CGAL::squared_distance(ci, cj) - (particle_sys[i].radius()+particle_sys[j].radius())*(particle_sys[i].radius()+particle_sys[j].radius());
			}
		}
	}
	void Circle_system::constraint_grad_at(const double *const x, double *constraint_grad)
	{
		if (!nonoverlap_constraint)		return;
		for (unsigned int i = 0; i < particle_sys.size(); i++)
		{
			Point_2 ci = particle_sys[i].center();
			for (unsigned int j = i+1; j < particle_sys.size(); j++)
			{
				Point_2 cj = particle_sys[j].center();

				*constraint_grad++ = 2*(ci.x()+x[2*i]-cj.x()-x[2*j]);
				*constraint_grad++ = 2*(ci.y()+x[2*i+1]-cj.y()-x[2*j+1]);

				*constraint_grad++ = 2*(cj.x()+x[2*j]-ci.x()-x[2*i]);
				*constraint_grad++ = 2*(cj.y()+x[2*j+1]-ci.y()-x[2*i+1]);
			}
		}
	}
	int Circle_system::optimize(double *trans, bool apply_constraints)
	{
		nonoverlap_constraint = apply_constraints;
		int  nStatus;
		/* variables that are passed to KNITRO */
		KTR_context *kc;
		int n, m, nnzJ, nnzH, objGoal, objType;
		int *cType, *jacIndexVars, *jacIndexCons;
		double obj, *lambda;
		double *xLoBnds, *xUpBnds, *xInitial, *cLoBnds, *cUpBnds;
		int i, j, k; // convenience variables

		/*problem size and mem allocation */
		n = particle_sys.size()*2;

		if (!nonoverlap_constraint)
			m = 0;
		else
			m = particle_sys.size()*(particle_sys.size()-1)/2;

		nnzJ = 4*m;
		nnzH = 0;
		//x = (double *)malloc(n * sizeof(double));
		lambda = (double *)malloc((m+n) * sizeof(double));

		xLoBnds = (double *)malloc(n * sizeof(double));
		xUpBnds = (double *)malloc(n * sizeof(double));
		xInitial = (double *)malloc(n * sizeof(double));
		cType        = (int    *)malloc(m * sizeof(int));
		cLoBnds      = (double *)malloc(m * sizeof(double));
		cUpBnds      = (double *)malloc(m * sizeof(double));
		jacIndexVars = (int    *)malloc(nnzJ * sizeof(int));
		jacIndexCons = (int    *)malloc(nnzJ * sizeof(int));

		/* objective type */
		objType = KTR_OBJTYPE_GENERAL;
		objGoal = KTR_OBJGOAL_MINIMIZE;
		/* bounds and constraints type */
		for (i = 0; i < particle_sys.size(); i++)
		{
			xLoBnds[2*i] = particle_sys[i].radius() + bounding_box.x_min() - particle_sys[i].center().x();
			xUpBnds[2*i] = bounding_box.x_max() - particle_sys[i].center().x() - particle_sys[i].radius();
			xLoBnds[2*i+1] = particle_sys[i].radius() + bounding_box.y_min() - particle_sys[i].center().y();
			xUpBnds[2*i+1] = bounding_box.y_max() - particle_sys[i].center().y() - particle_sys[i].radius();
		}
		for (j = 0; j < m; j++) {
			cType[j] = KTR_CONTYPE_QUADRATIC;
			cLoBnds[j] = 0.0;
			cUpBnds[j] = KTR_INFBOUND;
		}
		/* initial point */
		for (i = 0; i < n; i++)
			xInitial[i] = trans[i];

		/* sparsity pattern (here, of a full matrix) */
		k = 0;
		if (nonoverlap_constraint)
		{
			int r = 0;
			for (j = 0; j < particle_sys.size(); j++) 
				for (i = j+1; i < particle_sys.size(); i++)
				{
					jacIndexCons[k] = r;
					jacIndexVars[k] = 2*j;
					k++;
					jacIndexCons[k] = r;
					jacIndexVars[k] = 2*j+1;
					k++;
					jacIndexCons[k] = r;
					jacIndexVars[k] = 2*i;
					k++;
					jacIndexCons[k] = r;
					jacIndexVars[k] = 2*i+1;
					k++;
					r++;
				}
		}
		/* create a KNITRO instance */
		kc = KTR_new();
		if (kc == NULL)
			return -1000 ; // probably a license issue
		/* set options: automatic gradient and hessian matrix */
		if (KTR_set_int_param_by_name(kc, "bar_murule", 1) != 0)
			return -1000;
		if (KTR_set_int_param_by_name(kc, "gradopt", KTR_GRADOPT_EXACT) != 0)
			return -1000 ;
		if (KTR_set_int_param_by_name(kc, "hessopt", KTR_HESSOPT_LBFGS) != 0)
			return -1000 ;
		if (KTR_set_int_param_by_name(kc, "outlev", 0) != 0)
			return -1000 ;
		if (KTR_set_int_param_by_name(kc, "maxit", 500) != 0)
			return -1000 ;
		if (KTR_set_int_param_by_name(kc, "ms_enable", 1) != 0)
			return -1000;
		if (KTR_set_int_param_by_name(kc, "ms_terminate", 2) != 0)
			return -1000;
		/* register the callback function */
		if (KTR_set_func_callback(kc, &callback) != 0)
			return -1000 ;
		if (KTR_set_grad_callback(kc, &callback) != 0)
			return -1000 ;

		/* pass the problem definition to KNITRO */
		nStatus = KTR_init_problem(kc, n, objGoal, objType, xLoBnds, xUpBnds, m, cType, cLoBnds, cUpBnds,
									nnzJ, jacIndexVars, jacIndexCons, nnzH, NULL, NULL, xInitial, NULL);
		/* free memory (KNITRO maintains its own copy) */
		free(xLoBnds);	free(xUpBnds); free(xInitial);
		free(cType); free(cLoBnds);	free(cUpBnds);
		free(jacIndexVars);	free(jacIndexCons);

		nStatus = KTR_solve(kc, trans, lambda, 0, &obj, NULL, NULL, NULL, NULL, NULL, this);

		/* delete the KNITRO instance and primal/dual solution */
		KTR_free(&kc);
		free(lambda);
		return nStatus;
	}

	///////////////////////	Circle Unions	//////////////////////
	Circle_union_system::Circle_union_system(const std::vector<Circle_union>& particle_unions, const Container_box& bndbox) : particle_union_sys(particle_unions), System_potential_energy(bndbox)
	{
		nonoverlap_constraint = true;
		// the factor used in distance decay function
		dc = 0.0;
		unsigned int nb_circles = 0;
		for (unsigned int i = 0; i < particle_unions.size(); i++)
			for (unsigned int j = 0; j < particle_unions[i].size(); j++, nb_circles++)
				dc += particle_unions[i].circle_at(j).squared_radius();

		dc *= boost::math::constants::pi<double>();
		dc /= nb_circles;
		dc *= 4;
		vec_penalty_weight = 0.5;
		updated_circles.resize(particle_union_sys.size());
		for (unsigned int i = 0; i < particle_union_sys.size(); i++)
			updated_circles[i].resize(particle_union_sys[i].size());
	}
	void Circle_union_system::update_circles(const double *const x)
	{
		//std::for_each(updated_circles.begin(), updated_circles.end(), std::mem_fun_ref(&std::vector<Particle_circle>::clear));
		for (unsigned int i = 0; i < particle_union_sys.size(); i++)
		{
			//Particle_circle::Point_2 union_cent = particle_union_sys[i].centroid();
			//updated_circles[i].reserve(particle_union_sys[i].size());
			for (unsigned int j = 0; j < particle_union_sys[i].size(); j++)
				updated_circles[i][j] = centralized_transform(particle_union_sys[i].circle_at(j), x[3*i], x[3*i+1], x[3*i+2]);
				//updated_circles[i].push_back(centralized_transform(particle_union_sys[i].circle_at(j), x[3*i], x[3*i+1], x[3*i+2]));
		}
	}
	double Circle_union_system::system_energy_at(const double *const x)
	{
		update_circles(x);
		//double ge = gravity_energy_at(x);
		double me = molecular_energy_at(x);
		double ve = vector_align_energy(x);
		std::cout<<"me = "<<me<<", ve = "<<ve<<std::endl;
		//return ge + me + ve;
		return me + ve;
	}
	// x[n*2] is x-direction translation of the nth particle, x[2*n+1] is y-directional
	double Circle_union_system::molecular_energy_at(const double *const x)
	{
		double energy_sum = 0.0;
		
		for (unsigned int i = 0; i < particle_union_sys.size(); i++)
		{
			for (unsigned int j = i+1; j < particle_union_sys.size(); j++)
			{
				//if (i==j)	continue;
				for (unsigned int k = 0; k < updated_circles[i].size(); k++)
				{
					for (unsigned int l = 0; l < updated_circles[j].size(); l++)
					{
						double d2 = CGAL::squared_distance(updated_circles[i][k].center(), updated_circles[j][l].center());
						double rm = updated_circles[i][k].radius() + updated_circles[j][l].radius();
						double rm2_over_d2;
						if (d2 == 0.0)
							rm2_over_d2 = 0.0;
						else
							rm2_over_d2 = rm*rm/d2;
						double rm6_over_d6 = std::pow(rm2_over_d2, 3.0);

						////////////////////////////////////////////////
						//double w = CGAL::squared_distance(particle_union_sys[i].circle_at(k).center(), particle_union_sys[j].circle_at(l).center());
						//double decay_factor = std::pow(boost::math::constants::e<double>(), -w/dc);
						////////////////////////////////////////////////

						// (rm/(r0+r1))^6
						energy_sum += rm6_over_d6*(rm6_over_d6 - 2)/**decay_factor*/ ;
					}
				}
			}
		}
		energy_sum *= 2*epsilon; // due to symmetry
		
		return energy_sum;
	}
	
	double Circle_union_system::gravity_energy_at(const double *const x)
	{
		double gravity_energy = 0.0;
		for (unsigned int i = 0; i < particle_union_sys.size(); i++)
		{
			for (unsigned int j = 0; j < particle_union_sys[i].size(); j++)
			{
				const Particle_circle& uc = updated_circles[i].at(j);
				gravity_energy += uc.squared_radius()*(uc.center().y() - bounding_box.y_min());
			}
		}
		return boost::math::constants::pi<double>()*g*gravity_energy;
	}
	double Circle_union_system::vector_align_energy(const double *const x)
	{
		double vec_penalty = 0.0;
		for (unsigned int i = 0; i < particle_union_sys.size(); i++)
		{
			for (unsigned int j = i+1; j < particle_union_sys.size(); j++)
			{
				for (unsigned int k = 0; k < updated_circles[i].size(); k++)
				{
					for (unsigned int l = 0; l < updated_circles[j].size(); l++)
					{
						double w = CGAL::squared_distance(particle_union_sys[i].circle_at(k).center(), particle_union_sys[j].circle_at(l).center());
						//double w = CGAL::squared_distance(updated_circles[i].at(k).center(), updated_circles[j].at(l).center());
						double decay_factor = std::pow(boost::math::constants::e<double>(), -w/dc);
						vec_penalty += (1.0 - std::fabs(updated_circles[i][k].orientation_vector()*updated_circles[j][l].orientation_vector()))*decay_factor;
					}
				}
			}
		}
		return 2*vec_penalty_weight*vec_penalty;
	}
	void Circle_union_system::energy_gradient_at(const double *const x, double *gradient)
	{
		update_circles(x);
		for (unsigned int i = 0; i < particle_union_sys.size(); i++)
		{
			gradient[3*i] = gradient[3*i+1] = gradient[3*i+2] = 0.0;
			double fsini = std::sin(x[3*i]), fcosi = std::cos(x[3*i]);
			for (unsigned int j = 0; j < particle_union_sys.size(); j++)
			{
				if (j == i)		continue;
				//double fsinj = std::sin(x[3*j]), fcosj = std::cos(x[3*j]);
				for (unsigned int k = 0; k < particle_union_sys[i].size(); k++)
				{
					const Particle_circle& ik = particle_union_sys[i].circle_at(k);
					for (unsigned int l = 0; l < particle_union_sys[j].size(); l++)
					{
						const Particle_circle& jl = particle_union_sys[j].circle_at(l);
						// partial differential of d2 to tx
						double d2 = CGAL::squared_distance(updated_circles[i][k].center(), updated_circles[j][l].center());
						double rm = updated_circles[i][k].radius() + updated_circles[j][l].radius();
						double rm2_over_d2 = rm*rm/d2;
						double rm6_over_d6 = std::pow(rm2_over_d2, 3.0);

						///////////////////////////////////////////////////////
						//double w = CGAL::squared_distance(particle_union_sys[i].circle_at(k).center(), particle_union_sys[j].circle_at(l).center());
						//double decay_factor = std::pow(boost::math::constants::e<double>(), -w/dc);
						///////////////////////////////////////////////////////

						// (rm/(r0+r1))^6
						double common_factor = rm6_over_d6*(rm6_over_d6 - 1) ;
						common_factor /= d2;
						//common_factor *= decay_factor;

						double dx = updated_circles[i][k].center().x() - updated_circles[j][l].center().x();
						double dy = updated_circles[i][k].center().y() - updated_circles[j][l].center().y();
						gradient[3*i] += (2*dx*(-ik.center().x()*fsini - ik.center().y()*fcosi) + 2*dy*(ik.center().x()*fcosi - ik.center().y()*fsini))*common_factor;
						gradient[3*i+1] += 2*dx*common_factor;
						gradient[3*i+2] += 2*dy*common_factor;
					}
				}
			}
			gradient[3*i] *= -6*epsilon*2;
			gradient[3*i+1] *= -6*epsilon*2;
			gradient[3*i+2] *= -6*epsilon*2; // due to symmetry
		}
		// gradient of gravity energy
		for (unsigned int i = 0; i < particle_union_sys.size(); i++)
		{
			for (unsigned int j = 0; j < particle_union_sys[i].size(); j++)
			{
				//Particle_circle uc = centralized_transform(particle_union_sys[i].circle_at(j), particle_union_sys[i].centroid(), x[3*i], x[3*i+1], x[3*i+2]);
				gradient[3*i+2] += boost::math::constants::pi<double>()*particle_union_sys[i].circle_at(j).squared_radius()*g;
				gradient[3*i] += boost::math::constants::pi<double>()*g*particle_union_sys[i].circle_at(j).squared_radius()*
					(std::cos(x[3*i])*particle_union_sys[i].circle_at(j).center().x() -	std::sin(x[3*i]*particle_union_sys[i].circle_at(j).center().y()));
			}
		}			
		// gradient of vector alignment penalty
		for (unsigned int i = 0; i < particle_union_sys.size(); i++)
		{
			double vec_grad = 0.0;
			double fsini = std::sin(x[3*i]), fcosi = std::cos(x[3*i]);
			for (unsigned int j = 0; j < particle_union_sys.size(); j++)
			{
				if (j == i) continue;
				double fsinj = std::sin(x[3*j]), fcosj = std::cos(x[3*j]);
				for (unsigned int k = 0; k < particle_union_sys[i].size(); k++)
				{
					Vector_2 nik = particle_union_sys[i].circle_at(k).orientation_vector();
					for (unsigned int l = 0; l < particle_union_sys[j].size(); l++)
					{
						Vector_2 njl = particle_union_sys[j].circle_at(l).orientation_vector();
						double w = CGAL::squared_distance(particle_union_sys[i].circle_at(k).center(), particle_union_sys[j].circle_at(l).center());
						//double w = CGAL::squared_distance(updated_circles[i].at(k).center(), updated_circles[j].at(l).center());
						double decay_factor = std::pow(boost::math::constants::e<double>(), -w/dc);
						double temp = (-nik.x()*fsini-nik.y()*fcosi)*(njl.x()*fcosj-njl.y()*fsinj);
						temp += (nik.x()*fcosi-nik.y()*fsini)*(njl.x()*fsinj+njl.y()*fcosj);
						double crx_prod = (nik.x()*fcosi-nik.y()*fsini)*(njl.x()*fcosj-njl.y()*fsinj)+(nik.x()*fsini+nik.y()*fcosi)*(njl.x()*fsinj+njl.y()*fcosj);
						if (crx_prod > 0.0)
							temp = -temp;
						//gradient[3*i] += temp*decay_factor*vec_penalty_weight;
						vec_grad += temp*decay_factor*vec_penalty_weight;
					}
				}
			}
			//gradient[3*i] *= 2;
			gradient[3*i] += 2*vec_grad;
		}
	}

	void Circle_union_system::constraint_at(const double *const x, double *constraint_values)
	{

		for (unsigned int i = 0; i < updated_circles.size(); i++)
		{
			for (unsigned int j = 0; j < updated_circles[i].size(); j++)
			{
				*constraint_values++ = updated_circles[i][j].center().x();
				*constraint_values++ = updated_circles[i][j].center().y();
			}
		}
		if (nonoverlap_constraint)
		{
			for (unsigned int n = 0; n < nbpr.size(); n++)
			{
				unsigned int f = nbpr[n].first, s = nbpr[n].second;
				for (unsigned int i = 0; i < updated_circles[f].size(); i++)
				{
					for (unsigned int j = 0; j < updated_circles[s].size(); j++)
					{
						const Particle_circle& fi = updated_circles[f][i];
						const Particle_circle& sj = updated_circles[s][j];
						*constraint_values++ = CGAL::squared_distance(fi.center(), sj.center()) - (fi.radius() + sj.radius())*(fi.radius() + sj.radius());
					}
				}
			}
		}
	}
	void Circle_union_system::constraint_grad_at(const double *const x, double *constraint_grad)
	{
		for (unsigned int i = 0; i < particle_union_sys.size(); i++)
		{
			double fsin = std::sin(x[3*i]), fcos = std::cos(x[3*i]);
			for (unsigned int j = 0; j < particle_union_sys[i].size(); j++)
			{
				const Particle_circle& c = particle_union_sys[i].circle_at(j);
				*constraint_grad++ = -c.center().x()*fsin - c.center().y()*fcos;
				*constraint_grad++ = 1.0;
				*constraint_grad++ = 0.0;
				*constraint_grad++ = c.center().x()*fcos - c.center().y()*fsin;
				*constraint_grad++ = 0.0;
				*constraint_grad++ = 1.0;
			}
		}
		if (nonoverlap_constraint && nbpr.size() > 1)
		{
			for (unsigned int n = 0; n < nbpr.size(); n++)
			{
				unsigned int f = nbpr[n].first, s = nbpr[n].second;
				double fc = std::cos(x[3*f]), fs = std::sin(x[3*f]);
				double sc = std::cos(x[3*s]), ss = std::sin(x[3*s]);
				K::Aff_transformation_2 ft(fc, -fs, x[3*f+1], fs, fc, x[3*f+2]), st(sc, -ss, x[3*s+1], ss, sc, x[3*s+2]);
				for (unsigned int i = 0; i < particle_union_sys[f].size(); i++)
				{
					for (unsigned int j = 0; j < particle_union_sys[s].size(); j++)
					{
						const Particle_circle& fi = particle_union_sys[f].circle_at(i);
						const Particle_circle& sj = particle_union_sys[s].circle_at(j);
						Point_2 tip = ft(fi.center()), tjp = st(sj.center());
						*constraint_grad++ = 2*(tip.x() - tjp.x())*(-fi.center().x()*fs - fi.center().y()*fc) + 2*(tip.y() - tjp.y())*(fi.center().x()*fc - fi.center().y()*fs);
						*constraint_grad++ = 2*(tip.x() - tjp.x());
						*constraint_grad++ = 2*(tip.y() - tjp.y());
						*constraint_grad++ = 2*(tjp.x() - tip.x())*(-sj.center().x()*ss - sj.center().y()*sc) + 2*(tjp.y() - tip.y())*(sj.center().x()*sc - sj.center().y()*ss);
						*constraint_grad++ = 2*(tjp.x() - tip.x());
						*constraint_grad++ = 2*(tjp.y() - tip.y());
					}
				}
			}
		}
	}

	int Circle_union_system::optimize(double *trans, bool apply_constraints)
	{
		nonoverlap_constraint = apply_constraints;
		int  nStatus;
		/* variables that are passed to KNITRO */
		KTR_context *kc;
		int n, m, nnzJ, nnzH, objGoal, objType;
		int *cType, *jacIndexVars, *jacIndexCons;
		double obj, *x, *lambda;
		double *xLoBnds, *xUpBnds, *xInitial, *cLoBnds, *cUpBnds;
		//int i, j, k; // convenience variables

		/*problem size and mem allocation */
		n = particle_union_sys.size()*3;

		// for each circle, there is a constraint
		m = 0;
		for (unsigned int l = 0; l < particle_union_sys.size(); l++)
			m += particle_union_sys[l].size();
		m *= 2;
		nnzJ = m*3;

		//std::vector< std::pair<unsigned int, unsigned int> > nbpr;
		if (nonoverlap_constraint)
		{
			nbpr.clear();
			Circle_diagram cd;
			cd.insert(particle_union_sys);
			cd.neighbor_pairs(nbpr);

			for (unsigned int l = 0; l < nbpr.size(); l++)
			{
				unsigned int f = nbpr[l].first, s = nbpr[l].second;
				m += particle_union_sys[f].size()*particle_union_sys[s].size(); // each circle pair forms a constraint
				nnzJ += 2*3*particle_union_sys[f].size()*particle_union_sys[s].size(); // each constraint involves the tranformation of the two circles
			}
			//std::cout<<"End configuring KTR\n";
		}
		nnzH = 0;

		lambda = (double *)malloc((m+n) * sizeof(double));
		xLoBnds = (double *)malloc(n * sizeof(double));
		xUpBnds = (double *)malloc(n * sizeof(double));
		xInitial = (double *)malloc(n * sizeof(double));
		cType        = (int    *)malloc(m * sizeof(int));
		cLoBnds      = (double *)malloc(m * sizeof(double));
		cUpBnds      = (double *)malloc(m * sizeof(double));
		jacIndexVars = (int    *)malloc(nnzJ * sizeof(int));
		jacIndexCons = (int    *)malloc(nnzJ * sizeof(int));

		/* objective type */
		objType = KTR_OBJTYPE_GENERAL;
		objGoal = KTR_OBJGOAL_MINIMIZE;
		/* bounds and constraints type */
		for (unsigned int i = 0; i < n; i++) {
			xLoBnds[i] = -KTR_INFBOUND;
			xUpBnds[i] = KTR_INFBOUND;
		}
		for (unsigned int i = 0; i < particle_union_sys.size(); i++)
		{
			xLoBnds[i*3] = -boost::math::constants::pi<double>();
			xUpBnds[i*3] = boost::math::constants::pi<double>();
		}
		/* the circle union cannot go beyond the container */
		//int constraint_no = 0;
		double *ptrCLoBnds = cLoBnds, *ptrCUpBnds = cUpBnds;
		int *ptrCType = cType;
		for (unsigned int i = 0; i < particle_union_sys.size(); i++)
		{
			for (unsigned int j = 0; j < particle_union_sys[i].size(); j++)
			{
				double r = particle_union_sys[i].circle_at(j).radius();
				*ptrCType++ = KTR_CONTYPE_GENERAL;
				*ptrCLoBnds++ = bounding_box.x_min() + r;
				*ptrCUpBnds++ = bounding_box.x_max() - r;
				*ptrCType++ = KTR_CONTYPE_GENERAL;
				*ptrCLoBnds++ = bounding_box.y_min() + r;
				*ptrCUpBnds++ = bounding_box.y_max() - r;
			}
		}
		/* initial point */
		for (unsigned int i = 0; i < n; i++)
			xInitial[i] = trans[i];
		
		/* sparsity pattern (here, of a full matrix) */
		int *ptrCons = jacIndexCons, *ptrVars = jacIndexVars;
		int constraint_no = 0;
		for (unsigned int i = 0; i < particle_union_sys.size(); i++)
		{
			for (unsigned int j = 0; j < particle_union_sys[i].size(); j++)
			{
				/* x constraint for this circle */
				*ptrCons++ = constraint_no;
				*ptrVars++ = 3*i;
				*ptrCons++ = constraint_no;
				*ptrVars++ = 3*i+1;
				*ptrCons++ = constraint_no;
				*ptrVars++ = 3*i+2;
				constraint_no++;
				/* y constraint for this circle */
				*ptrCons++ = constraint_no;
				*ptrVars++ = 3*i;
				*ptrCons++ = constraint_no;
				*ptrVars++ = 3*i+1;
				*ptrCons++ = constraint_no;
				*ptrVars++ = 3*i+2;
				constraint_no++;
			}
		}
		
		if (nonoverlap_constraint)
		{
			for (unsigned int i = 0; i < nbpr.size(); i++)
			{
				unsigned int f = nbpr[i].first, s = nbpr[i].second;
				for (unsigned int j = 0; j < particle_union_sys[f].size(); j++)
				{
					for (unsigned int k = 0; k < particle_union_sys[s].size(); k++)
					{
						*ptrCType++ = KTR_CONTYPE_QUADRATIC;
						*ptrCLoBnds++ = 0.0;
						*ptrCUpBnds++ = KTR_INFBOUND;
						*ptrCons++ = constraint_no;
						*ptrVars++ = 3*f;
						*ptrCons++ = constraint_no;
						*ptrVars++ = 3*f + 1;
						*ptrCons++ = constraint_no;
						*ptrVars++ = 3*f + 2;
						*ptrCons++ = constraint_no;
						*ptrVars++ = 3*s;
						*ptrCons++ = constraint_no;
						*ptrVars++ = 3*s + 1;
						*ptrCons++ = constraint_no;
						*ptrVars++ = 3*s + 2;
						constraint_no++;
					}
				}
			}
		}
		
		/* create a KNITRO instance */
		kc = KTR_new();
		if (kc == NULL)
			return -1000 ; // probably a license issue
		if (KTR_set_int_param_by_name(kc, "algorithm", 2) != 0)
			return -1000;
		/* set options: automatic gradient and hessian matrix */
		if (KTR_set_int_param_by_name(kc, "bar_murule", 1) != 0) // Thus far, 0 and 1 are the best in experimental perspective
			return -1000;
		if (KTR_set_int_param_by_name(kc, "gradopt", KTR_GRADOPT_EXACT) != 0)
			return -1000 ;
		if (KTR_set_int_param_by_name(kc, "bar_maxbacktrack", 6) != 0)
			return -1000;
		if (KTR_set_int_param_by_name(kc, "bar_maxrefactor", 2) != 0)
			return -1000;
		if (KTR_set_int_param_by_name(kc, "hessopt", KTR_HESSOPT_SR1) != 0)
			return -1000 ;
		if (KTR_set_int_param_by_name(kc, "outlev", 1) != 0)
			return -1000 ;
		if (KTR_set_int_param_by_name(kc, "outmode", 0) != 0)
			return -1000;
		if (KTR_set_int_param_by_name(kc, "maxit", 0) != 0)
			return -1000 ;
		/* register the callback function */
		if (KTR_set_func_callback(kc, &callback) != 0)
			return -1000 ;
		if (KTR_set_grad_callback(kc, &callback) != 0)
			return -1000 ;
		// the following two setttings are for debug
		//if (KTR_set_int_param_by_name(kc, "newpoint", 3) != 0)
		//	return -1000;
		//if (KTR_set_newpoint_callback(kc, &update_interim) !=0 )
		//	return -1000;
		//int ms_option = 2;
		//if (nonoverlap_constraint)
		//	ms_option = 2;
		//if (KTR_set_int_param_by_name(kc, "ms_enable", 1) != 0)
		//	return -1000;
		//if (KTR_set_int_param_by_name(kc, "ms_maxsolves", 20) != 0)
		//	return -1000;
		//if (KTR_set_int_param_by_name(kc, "ms_terminate", 2) != 0)
		//	return -1000;
		/* pass the problem definition to KNITRO */
		nStatus = KTR_init_problem(kc, n, objGoal, objType, xLoBnds, xUpBnds, m, cType, cLoBnds, cUpBnds,
									nnzJ, jacIndexVars, jacIndexCons, nnzH, NULL, NULL, xInitial, NULL);
		/* free memory (KNITRO maintains its own copy) */
		free(xLoBnds);	free(xUpBnds); free(xInitial);
		free(cType); free(cLoBnds);	free(cUpBnds);
		free(jacIndexVars);	free(jacIndexCons);

		nStatus = KTR_solve(kc, trans, lambda, 0, &obj, NULL, NULL, NULL, NULL, NULL, this);

		/* delete the KNITRO instance and primal/dual solution */
		KTR_free(&kc);
		free(lambda);
		return nStatus;
	}


	int Circle_union_system::update_interim(const int evalRequestCode, const int n, const int m, const int nnzJ, 
							const int nnzH,	const double * const x,	const double * const lambda, 
							double * const obj, double * const c, double * const objGrad, double * const jac,
							double * const hessian,	double * const hessVector, void * userParams)
	{
		Circle_union_system *cus = static_cast<Circle_union_system*>(userParams);
		for (int i = 0; i < n; i++)
		{
			//if (i%3 == 0)
				//cus->ofs<<std::endl;
			cus->ofs<<x[i]<<' ';
		}
		cus->ofs<<std::endl;
		//static GLfloat color[] = {0.7f, 0.7f, 0.7f};
		////std::cout<<"Updating...\n";
		//for (unsigned int i = 0; i < cus->pp->size(); i++)
		//{
		//	//Packing_polygon tp = *cus->pp->at(i);
		//	cus->pp->at(i)->transform(Parameter(1.0, x[3*i], x[3*i+1], x[3*i+2]));
		//	cus->pp->at(i)->gl_draw(color);
		//}
		//glut_viewer_redraw();
	}
}